{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "781bf96d-cac4-40c8-a84c-aca80ae9f5f5",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1/1 [==============================] - 0s 171ms/step\n",
      "[[0.06822283 0.18360648 0.03141018 0.7167605 ]]\n",
      "right\n",
      "1/1 [==============================] - 0s 56ms/step\n",
      "[[0.6722949  0.01086661 0.03852108 0.27831736]]\n",
      "up\n",
      "1/1 [==============================] - 0s 138ms/step\n",
      "[[0.38175154 0.00156142 0.00514411 0.611543  ]]\n",
      "right\n",
      "1/1 [==============================] - 0s 55ms/step\n",
      "[[0.53530246 0.07248498 0.28482682 0.10738572]]\n",
      "up\n",
      "1/1 [==============================] - 0s 133ms/step\n",
      "[[0.9167128  0.01125866 0.06583839 0.00619021]]\n",
      "up\n",
      "1/1 [==============================] - 0s 134ms/step\n",
      "[[0.20862915 0.3547839  0.4204655  0.01612149]]\n",
      "left\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.00503082 0.8428686  0.11345924 0.03864128]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.15984823 0.39254165 0.431413   0.01619711]]\n",
      "left\n",
      "1/1 [==============================] - 0s 134ms/step\n",
      "[[0.16850197 0.39595807 0.42156145 0.01397855]]\n",
      "left\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.15811053 0.4051715  0.4219558  0.01476222]]\n",
      "left\n",
      "1/1 [==============================] - 0s 118ms/step\n",
      "[[3.4633261e-01 2.2221471e-01 4.3102798e-01 4.2468391e-04]]\n",
      "left\n",
      "1/1 [==============================] - 0s 55ms/step\n",
      "[[0.49370798 0.15184532 0.3539077  0.00053894]]\n",
      "up\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[1.09462725e-11 6.47616804e-01 3.01468790e-01 5.09143546e-02]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.00123745 0.79294455 0.03964456 0.16617349]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.00791393 0.7305128  0.08276902 0.17880423]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.08879346 0.296878   0.2695537  0.3447749 ]]\n",
      "right\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.27540576 0.44268414 0.13697442 0.14493568]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.63316464 0.1422422  0.1299228  0.09467033]]\n",
      "up\n",
      "1/1 [==============================] - 0s 131ms/step\n",
      "[[0.19826932 0.5196754  0.147369   0.13468638]]\n",
      "down\n",
      "1/1 [==============================] - 0s 55ms/step\n",
      "[[0.05092843 0.7612735  0.11390253 0.07389561]]\n",
      "down\n",
      "1/1 [==============================] - 0s 60ms/step\n",
      "[[0.19588108 0.5294081  0.15032029 0.1243906 ]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.5919773  0.16546968 0.13104844 0.11150458]]\n",
      "up\n",
      "1/1 [==============================] - 0s 135ms/step\n",
      "[[0.1254459  0.6212514  0.11489913 0.13840355]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.04982096 0.7564456  0.11349355 0.08023991]]\n",
      "down\n",
      "1/1 [==============================] - 0s 54ms/step\n",
      "[[0.19506763 0.5120016  0.15242709 0.1405037 ]]\n",
      "down\n",
      "1/1 [==============================] - 0s 55ms/step\n",
      "[[0.4025612  0.28368896 0.09570873 0.21804105]]\n",
      "up\n"
     ]
    }
   ],
   "source": [
    "#!/usr/bin/python  \n",
    "# coding=utf-8  \n",
    "#本段代码实现树莓派智能小车的红外避障效果\n",
    "#代码使用的树莓派GPIO是用的BOARD编码方式。 \n",
    "import RPi.GPIO as GPIO\n",
    "#电机控制器\n",
    "from motor import Motor\n",
    "#红外距离传感器\n",
    "from infrared_sensor import InfraredSensor\n",
    "#超声波避障\n",
    "from adafruit.Adafruit import Adafruit\n",
    "\n",
    "import time\n",
    "import sys\n",
    "import tensorflow as tf\n",
    "import numpy as np\n",
    "import os\n",
    "\n",
    "#动作常量\n",
    "ACTION_UP = 0\n",
    "ACTION_DOWN = 1\n",
    "ACTION_LEFT = 2\n",
    "ACTION_RIGHT = 3\n",
    "\n",
    "#电机控制类\n",
    "motor = Motor()\n",
    "#超声波\n",
    "adafruit = Adafruit()\n",
    "\n",
    "#加载自动驾驶模型\n",
    "model = tf.keras.models.load_model(sys.path[0]+'/ai/model.h5')\n",
    "\n",
    "#采集距离\n",
    "def collect_dists(adafruit,is_opposite=False,step_angle=0.25):\n",
    "    dists = [0,0,0,0,0]\n",
    "    \n",
    "    #从右边开始采集\n",
    "    if is_opposite == False:\n",
    "        for i in range(5):\n",
    "            dists[i]=adafruit.angel_dist(step_angle*i)\n",
    "            # time.sleep(0.1)\n",
    "    #从左边开始采集\n",
    "    else:\n",
    "        for i in range(4,-1,-1):\n",
    "            dists[i]=adafruit.angel_dist(step_angle*i)\n",
    "            # time.sleep(0.1)\n",
    "    return dists\n",
    "    \n",
    "def setup():\n",
    "    GPIO.setwarnings(False)\n",
    "    GPIO.setmode(GPIO.BCM)       # Numbers GPIOs by physical location\n",
    "\n",
    "    #初始化电机\n",
    "    motor.init()\n",
    "    #初始化超声波\n",
    "    adafruit.init()\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    setup()\n",
    "\n",
    "    #启动电机\n",
    "    motor.start()\n",
    "    \n",
    "    is_opposite = False\n",
    "    \n",
    "    try:\n",
    "        while True:\n",
    "            #采集超声波距离\n",
    "            dists = collect_dists(adafruit,is_opposite)\n",
    "            #调转采集的方向\n",
    "            is_opposite = not is_opposite\n",
    "            \n",
    "            # 利用模型对测试数据进行预测\n",
    "            dists = np.array([dists],np.float64)\n",
    "            dists = dists / 500.0\n",
    "            y = model.predict(dists)\n",
    "            print(y)\n",
    "            #计算执行动作\n",
    "            action = np.argmax(y)\n",
    "            \n",
    "            # 执行动作\n",
    "            if action == ACTION_UP:\n",
    "                print (\"up\")\n",
    "                motor.up(20,0)\n",
    "            elif action == ACTION_DOWN:\n",
    "                print (\"down\")\n",
    "                motor.down(30,0.5)\n",
    "                motor.stop(0.01)\n",
    "            elif action == ACTION_LEFT:\n",
    "                print (\"left\")\n",
    "                motor.left(30,0.5)\n",
    "                motor.stop(0.01)\n",
    "            elif action == ACTION_RIGHT:\n",
    "                print (\"right\")\n",
    "                motor.right(30,0.5)\n",
    "                motor.stop(0.01)\n",
    "                \n",
    "    except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the child program destroy() will be  executed.\n",
    "        GPIO.cleanup()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dd4f34d0-8093-43ac-b4b2-60315346927a",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "769eacc1-837e-43be-b86d-35f9819ff836",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "tensorflow_env",
   "language": "python",
   "name": "tensorflow_env"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
